#include "framework.h"

namespace poly
{
	/////////////////////////////
	// POLYGON-POLYGON FUNCTIONS
	/////////////////////////////

	static void projectOnAxis( float3 const & axis, Polygon const & poly,
		float & min, float & max, float3 & minV, float3 & maxV )
	{
		// initialize minmax variables
		min = dot( poly[0], axis );
		max = dot( poly[0], axis );

		minV = poly[0];
		maxV = poly[0];

		for( Polygon::const_iterator v = poly.begin();
			v != poly.end();
			v++ )
		{
			float vDotAxis = dot( *v, axis );
			if( vDotAxis < min )
			{
				min = vDotAxis;
				minV = *v;
			}
			else if( vDotAxis > max )
			{
				max = vDotAxis;
				maxV = *v;
			}
		}
	}

	// determines whether there is an overlap between A and B on axis.
	// AXIS MUST BELONG TO POLYGON A
	// CONTACT POINT RETURNED FROM POLYGON B
	// d[out]: distance to move B on axis to resolve overlap
	// seperation[out]: vector to move B to resolve overlap
	// intersection point from b[out]: minima and maxima to be used in collision resolution
	static bool intersectOnAxis(Polygon const &A, Polygon const & B, float3 axis,
		float &d, float3 & seperation,
		float3 &intersectPt )
	{
		float minA, maxA, minB, maxB;
		float3 minVA, maxVA, minVB, maxVB;

		projectOnAxis( axis, A, minA, maxA, minVA, maxVA );
		projectOnAxis( axis, B, minB, maxB, minVB, maxVB );

		float d0 = maxB - minA;
		float d1 = maxA - minB;

		if( d0 > 0 && d1 > 0 ) // if overlapped
		{
			// decide on intersection location
			if( d0 < d1 )
			{
				d = -d0;
				seperation = axis * d;
				intersectPt = maxVB;
			}
			else
			{
				d = +d1;
				seperation = axis * d;
				intersectPt = minVB;
			}

			return true;
		}
		return false;
	}

	static bool intersectOnEdges( Polygon const & edges, Polygon const & P, bool invert,
		Contact& contact, bool & intersecting, float & d )
	{
		for( size_t i = 0, j = edges.size()-1; i < edges.size(); j = i, i++ )
		{
			float3 axis = edges[i]-edges[j];
			axis = normalize( cross( axis, float3(0,0,1) ) );

			float axisD;
			float3 axisSeperation;
			float3 axisPoint;

			bool isIntersectOnAxis = intersectOnAxis(
				edges, P, axis,
				axisD, axisSeperation,
				axisPoint );

			if( !isIntersectOnAxis ) return false;

			if( !intersecting || fabs(axisD) < fabs(d) ) // if not previously intersecting
				// or this intersection is shallower than pervious intersection
			{
				float sign = invert?-1:+1;
				intersecting = true;
				d = axisD;
				contact.seperation = sign*axisSeperation;
				contact.point = axisPoint;
				contact.axis = sign*axis;
			}
		}

		return intersecting;
	}

	bool intersect( Contact& contact,
		Polygon const & A, Polygon const & B )
	{
		bool intersecting = false;
		float d;

		if( !intersectOnEdges( A, B, false, contact, intersecting, d ) ) return false;
		if( !intersectOnEdges( B, A, true, contact, intersecting, d ) ) return false;

		return intersecting;
	}

	bool intersect( float3& intersection, Segment const & segment1, Segment const & segment2 )
	{
		float3 const & A1 = segment1.point0;
		float3 const & A2 = segment1.point1;
		float3 const & B1 = segment2.point0;
		float3 const & B2 = segment2.point1;

		double r, s;
		double denominator = (A2.x - A1.x) * (B2.y - B1.y) - (A2.y - A1.y) * (B2.x - B1.x);

		if (denominator == 0)	return false;

		double numeratorR = (A1.y - B1.y) * (B2.x - B1.x) - (A1.x - B1.x) * (B2.y - B1.y);
		r = numeratorR / denominator;

		double numeratorS = (A1.y - B1.y) * (A2.x - A1.x) - (A1.x - B1.x) * (A2.y - A1.y);
		s = numeratorS / denominator;

		if (r < 0 || r > 1 || s < 0 || s > 1)	return false;

		intersection.x = (float)(A1.x + (r * (A2.x - A1.x)));
		intersection.y = (float)(A1.y + (r * (A2.y - A1.y)));
		intersection.z = 0.0f;
		return true;
	}

	bool intersect( Contact& contact, float3 center1, float radius1, float3 center2, float radius2)
	{
		contact.axis = normalize(center2-center1);
		contact.seperation = (radius1+radius2-length(center1-center2))*contact.axis;
		contact.point = center1 + contact.axis * radius1 - contact.seperation/2;

		if(length(contact.point-center1) <= radius1) return true;

		else return false;
	}

	Polygon conjunction( Polygon const & A, Line const & L )
	{
		Polygon out;
		for( size_t i = A.size()-1, j = 0; j < A.size(); i = j, j++ )
		{
			float doti = dot(A[i]-L.point, L.normal );
			float dotj = dot(A[j]-L.point, L.normal );

			if( doti > 0 )
			{
				out.push_back( A[i] );
				if( dotj < 0 ) // crossing from +ve halfspace to -ve halfspace
				{
					float3 Ax = -doti/(dotj-doti)*(A[j]-A[i]) + A[i];
					out.push_back( Ax );
				}
			}
			else
			{
				if( dotj > 0 ) // crossing from -ve halfspace to +ve halfspace
				{
					float3 Ax = -doti/(dotj-doti)*(A[j]-A[i]) + A[i];
					out.push_back( Ax );
				}
			}
		}

		return out;
	}

	Polygon conjunction( Polygon const & A, Polygon const & B )
	{
		Polygon out = B;
		for( size_t i = A.size()-1, j = 0; j < A.size(); i = j, j++ )
		{
			if( out.size() == 0 ) break;

			Line L;
			L.point = A[i];
			float3 tangent = A[j]-A[i];
			L.normal = float3( -tangent.y, +tangent.x, 0 );

			out = conjunction( out, L );
		}

		return out;
	}





	/////////////////////////////
	// POLYGON-CIRCLE FUNCTIONS
	/////////////////////////////
	static void projectOnAxis( float3 const & axis, float3 center, float radius,
	float & min, float & max, float3 & minV, float3 & maxV )
	{
		float vDotAxis_max = dot( center+(radius*axis), axis );
		float vDotAxis_min = dot( center-(radius*axis), axis );

		min = vDotAxis_min;
		minV = center-(radius*axis);

		max = vDotAxis_max;
		maxV = center+(radius*axis);
	}

	static bool intersectOnAxis(Polygon const &A, float3 center, float radius, float3 axis,
		float &d, float3 & seperation,
		float3 &intersectPt )
	{
		float minA, maxA, minB, maxB;
		float3 minVA, maxVA, minVB, maxVB;

		projectOnAxis( axis, A, minA, maxA, minVA, maxVA );
		projectOnAxis( axis, center, radius, minB, maxB, minVB, maxVB );

		float d0 = maxB - minA;
		float d1 = maxA - minB;

		if( d0 > 0 && d1 > 0 ) // if overlapped
		{
			// decide on intersection location
			if( d0 < d1 )
			{
				d = -d0;
				seperation = axis * d;
				intersectPt = maxVB;
			}
			else
			{
				d = +d1;
				seperation = axis * d;
				intersectPt = minVB;
			}

			return true;
		}
		return false;
	}

	static bool intersectOnEdges( Polygon const & edges, float3 center, float radius, bool invert,
		Contact& contact, bool & intersecting, float & d )
	{
		for( size_t i = 0, j = edges.size()-1; i < edges.size(); j = i, i++ )
		{
			float3 axis = edges[i]-edges[j];
			axis = normalize( cross( axis, float3(0,0,1) ) );

			float axisD;
			float3 axisSeperation;
			float3 axisPoint;

			bool isIntersectOnAxis = intersectOnAxis(
				edges, center, radius, axis,
				axisD, axisSeperation,
				axisPoint );

			if( !isIntersectOnAxis ) return false;

			if( !intersecting || fabs(axisD) < fabs(d) ) // if not previously intersecting
				// or this intersection is shallower than pervious intersection
			{
				float sign = invert?-1:+1;
				intersecting = true;
				d = axisD;
				contact.seperation = sign*axisSeperation;
				contact.point = axisPoint;
				contact.axis = sign*axis;
			}
		}

		return intersecting;
	}

	bool intersect( Contact& contact,
		Polygon const & A, float3 center, float radius )
	{
		bool intersecting = false;
		float d;

		float3 toCenter;
		float3 edge_after;
		float3 edge_before;
		float3 sep=contact.seperation;

		for(int i=0; i<A.size(); i++)
		{
			toCenter = center - A[i];
			edge_after = A[(i+1)%A.size()] - A[i];
			edge_before = A[((i-1)+A.size()) % A.size()] - A[i];
			bool rh1 = rightHanded( zero3(), edge_before, toCenter );
			bool rh2 = rightHanded( zero3(), toCenter, edge_after );

			if(
				(dot(toCenter,edge_after) <= 0) &&
				(dot(toCenter,edge_before) <= 0) )
			{
				if( lengthSq(toCenter)<radius*radius )
				{
					contact.seperation = radius*normalize(toCenter)-toCenter;
					contact.axis	   = normalize(sep);
					contact.point	   = contact.seperation/2 + A[i];
					return true;
				}
				else
					return false;
			}
		}

		if( !intersectOnEdges( A, center, radius, false, contact, intersecting, d ) ) 
			return false;

		return intersecting;
	}





	//////////////////////////////
	// POLYGON-POLYLINE FUNCTIONS
	//////////////////////////////
	static void projectOnAxis( float3 const & axis, Segment const & seg,
		float & min, float & max, float3 & minV, float3 & maxV )
	{
		// initialize minmax variables
		min = dot( seg.point0, axis );
		max = dot( seg.point0, axis );

		minV = seg.point0;
		maxV = seg.point0;

		float vDotAxis = dot( seg.point1, axis );
		if( vDotAxis < min )
		{
			min = vDotAxis;
			minV = seg.point1;
		}
		else if( vDotAxis > max )
		{
			max = vDotAxis;
			maxV = seg.point1;
		}
	}

	static bool intersectOnAxis(Polygon const &A, Segment const & B, float3 axis,
		float &d, float3 & seperation,
		float3 &intersectPt )
	{
		float minA, maxA, minB, maxB;
		float3 minVA, maxVA, minVB, maxVB;

		projectOnAxis( axis, A, minA, maxA, minVA, maxVA );
		projectOnAxis( axis, B, minB, maxB, minVB, maxVB );

		float d0 = maxB - minA;
		float d1 = maxA - minB;

		if( d0 > 0 && d1 > 0 ) // if overlapped
		{
			// decide on intersection location
			if( d0 < d1 )
			{
				d = -d0;
				seperation = axis * d;
				intersectPt = maxVB;
			}
			else
			{
				d = +d1;
				seperation = axis * d;
				intersectPt = minVB;
			}

			return true;
		}
		return false;
	}

	static bool intersectOnEdges( Polygon const & edges, Segment const & P, bool invert,
		Contact& contact, bool & intersecting, float & d )
	{
		for( size_t i = 0, j = edges.size()-1; i < edges.size(); j = i, i++ )
		{
			float3 axis = edges[i]-edges[j];
			axis = normalize( cross( axis, float3(0,0,1) ) );

			float axisD;
			float3 axisSeperation;
			float3 axisPoint;

			bool isIntersectOnAxis = intersectOnAxis(
				edges, P, axis,
				axisD, axisSeperation,
				axisPoint );

			if( !isIntersectOnAxis ) return false;

			if( !intersecting || fabs(axisD) < fabs(d) ) // if not previously intersecting
				// or this intersection is shallower than pervious intersection
			{
				float sign = invert?-1:+1;
				intersecting = true;
				d = axisD;
				contact.seperation = sign*axisSeperation;
				contact.point = axisPoint;
				contact.axis = sign*axis;
			}
		}

		return intersecting;
	}

	static bool intersectOnEdges( Segment const & edges, Polygon const & P, bool invert,
		Contact& contact, bool & intersecting, float & d )
	{
		float3 axis = edges.point1-edges.point0;
		axis = normalize( cross( axis, float3(0,0,1) ) );

		float axisD;
		float3 axisSeperation;
		float3 axisPoint;

		bool isIntersectOnAxis = intersectOnAxis(
			P, edges, axis,
			axisD, axisSeperation,
			axisPoint );

		if( !isIntersectOnAxis ) return false;

		if( !intersecting || fabs(axisD) < fabs(d) ) // if not previously intersecting
			// or this intersection is shallower than pervious intersection
		{
			float sign = invert?-1:+1;
			intersecting = true;
			d = axisD;
			contact.seperation = sign*axisSeperation;
			contact.point = axisPoint;
			contact.axis = sign*axis;
		}

		return intersecting;
	}

	bool intersect( Contact& contact,
		Polygon const & A, Segment const & B )
	{
		bool intersecting = false;
		float d;

		if( !intersectOnEdges( A, B, false, contact, intersecting, d ) ) return false;
		if( !intersectOnEdges( B, A, false, contact, intersecting, d ) ) return false;

		return intersecting;
	}

	void intersect( ContactList& contacts,
		Polygon const & A, PolyLine const & B )
	{
		contacts.clear();
		for( int i = 0; i < B.size() - 1; i++ )
		{
			Contact contact_i;
			Segment seg = {B[i], B[i+1]};
			if( intersect( contact_i, A, seg ) )
				contacts.push_back( contact_i );
		}
	}
} // end of poly
